
#include "IAlgorithm.h"
#include "IContinuousConvexPlaneCast.h"
#include "IGjkPairDetector.h"
#include "IDetectorResult.h"
#include "BGridPlane"
#include "member_BShape.h"

IContinuousConvexPlaneCast::IContinuousConvexPlaneCast(BShape* convexA, BGridPlane* plane)
{
	_shapeA = convexA;
	_staticPlane = plane;
}

void IContinuousConvexPlaneCast::computeClosestPoints(const BMatrix& matrixA, const BMatrix& matrixB, IDetectorResult& result)
{
	const BVector& normal = _staticPlane->normal();
	const BReal& constant = _staticPlane->constant();

	BMatrix convexInPlane = matrixB.inverse() * matrixA;
	BMatrix planeInConvex = matrixA.inverse() * matrixB;

	BVector vertex = shape_member(_shapeA)->supportVertex(planeInConvex.mult3(-normal));

	BVector vertexInPlane = convexInPlane * vertex;
	BReal distance = (normal.dot(vertexInPlane) - constant);

	BVector contactPoint = vertexInPlane - distance * normal;
	BVector pointB = matrixB * contactPoint;
	BVector normalB = matrixB.mult3(normal);

	result.addContactPoint(pointB, normalB, distance);
}

bool IContinuousConvexPlaneCast::calcTimeOfImpact(const BMatrix& sourceA, const BMatrix& targetA, const BMatrix& sourceB, const BMatrix& targetB, CastResult& caseResult)
{
	IDetectorResult result;
	computeClosestPoints(sourceA, sourceB, result);
	if (!result.hasResult)
		return false;

	BVector linVelA, angVelA;
	CalculateVelocity(sourceA, targetA, linVelA, angVelA);

	BReal boundingRadiusA = _shapeA->box().radius();

	BReal maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA;
	BVector relLinVel = -linVelA;

	BReal relLinVelocLength = relLinVel.length();
	if ((relLinVelocLength + maxAngularProjectedVelocity) == 0)
		return false;

	BReal dist = result.distance;
	BVector point = result.pointB;
	BVector normal = result.normalB;
	BReal projectedLinearVelocity = relLinVel.dot(normal);
	if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= REAL_EPSILON)
		return false;

	int numIter = 0;
	BReal fraction = 0;
	while (dist > REAL_EPSILON)
	{
		projectedLinearVelocity = relLinVel.dot(normal);
		if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= REAL_EPSILON)
			return false;

		fraction += dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
		if (fraction < 0 || fraction > 1)
			return false;

		BMatrix interpMatrixA = IntegrateTransform(sourceA, linVelA, angVelA, fraction);
		computeClosestPoints(interpMatrixA, sourceB, result);
		if (result.hasResult)
		{
			dist = result.distance;
			point = result.pointB;
			normal = result.normalB;
		}
		else
		{
			return false;
		}
		if (numIter++ > 64)
		{
			return false;
		}
	}

	caseResult.fraction = fraction;
	caseResult.distance = dist;
	caseResult.normal = normal;
	caseResult.point = point;
	return true;
}
